library IEEE;
use IEEE.STD_LOGIC_1164.ALL;
use IEEE.numeric_std.all;
use work.state_pkg.ALL;


entity linesearcher is
	port ( clk 	:	in	std_logic;
		reset 	:	in	std_logic;
		
		sensor_l :	in	std_logic;
		sensor_m :	in	std_logic;
		sensor_r :	in	std_logic;
		max_count : in unsigned(31 downto 0);
		
		continu	:	out	std_logic;
		rover_direction	:	out	direction_type;
		dbm		: out	string(2 downto 1)
		);		
end linesearcher;

architecture Behavioral of linesearcher is

	type linesearcher_state is (find, pass, left, right, found, stop);
	
       signal state, new_state:  linesearcher_state;
		 signal last_input, new_last_input, sensor: std_logic_vector(2 downto 0);
		 signal count : unsigned(31 downto 0);
		 
begin
    sensor <= sensor_l & sensor_m & sensor_r;
    
    process (clk)
        begin
               if (rising_edge (clk)) then
                   if (reset = '1') then
                        state <= find;
								        last_input <= "111";
								        count <= to_unsigned(0, 32);
								   elsif (count >= max_count) then
								        state <= stop;
                   else
                        state <= new_state;
							          new_last_input <= sensor;
							          if (sensor = "111") then
							             count <= count + 1;
							          else 
							             count <= to_unsigned(0, 32);
							          end if; 
									 
								      	 if new_last_input = "111" then
									      		last_input <= last_input;
									      else
											     last_input <= new_last_input;
									      end if;
                   end if;
               end if;
        end process;
    

    process (state, sensor)
        begin
          
               case state is
                when find =>
                   rover_direction <= straight;
							  dbm <= "FI";
								continu <= '1';
								if (sensor = "111") then
                              new_state <= find;
								elsif (sensor = "101") then
										new_state <= found;
								else
										new_state <= pass;
								end if;
								
						 when pass =>
								rover_direction <= straight;
								dbm <= "PA";
								continu <= '1';

								if (sensor /= "111") then
										new_state <= pass;
								elsif (last_input = "001" or last_input = "011") then
										new_state <= left;
								else
										new_state <= right;
								end if;
								
						 when left =>
								rover_direction <= hard_left;
								dbm <= "LE";
								continu <= '1';
								
								if (sensor /= "101") then
										new_state <= left;
								else
										new_state <= found;
								end if;
								
						 when right =>
								rover_direction <= hard_right;
								dbm <= "RI";
								continu <= '1';
								
								if (sensor /= "101") then
										new_state <= right;
								else
										new_state <= found;
								end if;
						
						 when found =>
								rover_direction <= stop;
								dbm <= "FO";
								continu <= '0';
								new_state <= found;
								
						 when stop =>
								rover_direction <= stop;
								dbm <= "St";
								continu <= '1';
								new_state <= stop;
																															
                end case;
            end process;

end Behavioral;

